//
//  twi.c
//  NMEA
//
//  Created by Hans Johnson on 2014-03-22.
//
//

#include <stdio.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include "avr_compiler.h"
#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"
#include "semphr.h"
#include "twi.h"

xSemaphoreHandle Twi_Mutex;
xSemaphoreHandle Twi_Sync;

// Function to initialize
void twi_init()
{
    // Clear the TWID_CTRL register
    TWID.CTRL = 0x00;
    // Enable TWI on port D, and set the interrupt to medium  level
    TWID.MASTER.CTRLA |= (1<<TWI_MASTER_ENABLE_bp)|TWI_MASTER_INTLVL_MED_gc;
    // set the BAUD Rate
    TWID.MASTER.BAUD = TWI_BAUD_DIV;
    
    // Setup the semaphore For bus exclusion
    Twi_Mutex = xSemaphoreCreateMutex();
    
    // Setup the semaphore for interrupt synchronization
    Twi_Sync = xSemaphoreCreateBinary();
    
};

// Blocking Function to send data out over I2C
int8_t twi_send(uint8_t addr, void *payload, uint8_t length)
{
    // itterator
    uint8_t i;
    
    // Snag the Mutex
    xSemaphoreTake(Twi_Mutex, portMAX_DELAY);
    // Set the address
    TWID.MASTER.ADDR = addr;
    // Itterate over the string to send
    for (i = 0; i < length; i++) {
        // Put the next byte of data into the data register
        TWID.MASTER.DATA = *(char*)(payload+i);
        // 
    }
    return 0;
};

ISR( TWID_TWIM_vect)
{
    xSemaphoreGiveFromISR(Twi_Sync, NULL);
}